#include "imu_controller.hpp"
#include "endian.h"

imu_controller::imu_controller(std::shared_ptr<iolayer::layer_socket> sock, int device_id)
{
	this->sock = sock;
	this->device_id = device_id;
	return;
}

imu_frame_id imu_controller::read_frame()
{
	uint32_t id;
	int dlc, dlc_exp;

	if (this->sock->read(&id, &dlc, this->recv_buf) <= 0)
	{
		return imu_frame_id::none;
	}
	switch (id - this->device_id)
	{
		case imu_frame_id::accel:
			dlc_exp = 6;
			break;
		case imu_frame_id::angula:
			dlc_exp = 6;
			break;
		case imu_frame_id::euler:
			dlc_exp = 6;
			break;
		case imu_frame_id::quad:
			dlc_exp = 8;
			break;
		case imu_frame_id::pascal:
			dlc_exp = 8;
			break;
		default:
			return imu_frame_id::other;
	}
	if (dlc_exp != dlc)
	{
		return imu_frame_id::none;
	}
	return (imu_frame_id)(id - this->device_id);
}

int imu_controller::get_data_as_accel(int &x, int &y, int &z)
{
	int16_t *data = (int16_t *)this->recv_buf;
	x = (int16_t)le16toh(data[0]);
	y = (int16_t)le16toh(data[1]);
	z = (int16_t)le16toh(data[2]);
	return 0;
}

int imu_controller::get_data_as_angula(int &x, int &y, int &z)
{
	int16_t *data = (int16_t *)this->recv_buf;
	x = (int16_t)le16toh(data[0]);
	y = (int16_t)le16toh(data[1]);
	z = (int16_t)le16toh(data[2]);
	return 0;
}

int imu_controller::get_data_as_euler(int &roll, int &pitch, int &yaw)
{
	int16_t *data = (int16_t *)this->recv_buf;
	roll = (int16_t)le16toh(data[0]);
	pitch = (int16_t)le16toh(data[1]);
	yaw = (int16_t)le16toh(data[2]);
	return 0;
}

int imu_controller::get_data_as_quad(int &qw, int &qx, int &qy, int &qz)
{
	int16_t *data = (int16_t *)this->recv_buf;
	qw = (int16_t)le16toh(data[0]);
	qx = (int16_t)le16toh(data[1]);
	qy = (int16_t)le16toh(data[2]);
	qz = (int16_t)le16toh(data[3]);
	return 0;
}

int imu_controller::get_data_as_pascal(int &pa)
{
	return (int32_t)(le32toh(*(uint32_t *)this->recv_buf));
}

int imu_controller::get_data_as_bytes(uint8_t data[8])
{
	memcpy(data, this->recv_buf, 8);
	return 0;
}

/**
 * @brief write a sdo config frame
 *
 * @param id id, usually be 0x600
 * @param sdo sdo command, 0x23 to write 4 bytes, 0x2B to write 2 bytes
 * @param index sdo index, 16 bits
 * @param sub sdo sub index, 8 bits
 * @param data data, 32 bits
 * @return int >0: success, <0: failed
 */
int imu_controller::write_frame(int id, int sdo, int index, int sub, int data)
{
	uint8_t buf[8] = {0};
	buf[0] = sdo;
	*(uint16_t *)(buf + 1) = htole16(index);
	buf[3] = sub;
	*(uint32_t *)(buf + 4) = htole32(data);
	return this->sock->write(id + this->device_id, 8, buf);
}

/**
 * @brief config device node id
 *
 * @param id id, from 1 to 64
 * @return int >0: success, <0: failed
 */
int imu_controller::write_devid(int id)
{
	return this->write_frame(0x600, 0x23, 0x2101, 0x00, id);
}

/**
 * @brief config acceleration output frequency level
 *
 * @param level 0: off, 0x05: 200Hz, 0x0A: 100Hz, 0x14: 50Hz, 0x32: 20Hz, 0x64: 10Hz
 * @return int >0: success, <0: failed
 */
int imu_controller::write_freq_accel(int level)
{
	return this->write_frame(0x600, 0x2B, 0x1800, 0x05, level);
}

/**
 * @brief config angular speed output frequency level
 *
 * @param level 0: off, 0x05: 200Hz, 0x0A: 100Hz, 0x14: 50Hz, 0x32: 20Hz, 0x64: 10Hz
 * @return int >0: success, <0: failed
 */
int imu_controller::write_freq_angula(int level)
{
	return this->write_frame(0x600, 0x2B, 0x1801, 0x05, level);
}

/**
 * @brief config euler angle output frequency level
 *
 * @param level 0: off, 0x05: 200Hz, 0x0A: 100Hz, 0x14: 50Hz, 0x32: 20Hz, 0x64: 10Hz
 * @return int >0: success, <0: failed
 */
int imu_controller::write_freq_euler(int level)
{
	return this->write_frame(0x600, 0x2B, 0x1802, 0x05, level);
}

/**
 * @brief config quaterion output frequency level
 *
 * @param level 0: off, 0x05: 200Hz, 0x0A: 100Hz, 0x14: 50Hz, 0x32: 20Hz, 0x64: 10Hz
 * @return int >0: success, <0: failed
 */
int imu_controller::write_freq_quad(int level)
{
	return this->write_frame(0x600, 0x2B, 0x1803, 0x05, level);
}

/**
 * @brief config air pressure output frequency level
 *
 * @param level 0: off, 0x05: 200Hz, 0x0A: 100Hz, 0x14: 50Hz, 0x32: 20Hz, 0x64: 10Hz
 * @return int >0: success, <0: failed
 */
int imu_controller::write_freq_pascal(int level)
{
	return this->write_frame(0x600, 0x2B, 0x1804, 0x05, level);
}